#include "Arduino.h"
#include "MotorControl.h" //include the declaration for this class

//int driveDirection = LOW;
//int turnDirection = HIGH;

MotorControl::MotorControl(int enable1, int phase1, int enable2, int phase2, int mode) { 
  pinMode(mode, OUTPUT); 
  pinMode(enable1, OUTPUT);
  pinMode(enable2, OUTPUT);
  pinMode(phase1, OUTPUT);
  pinMode(phase2, OUTPUT);

  _enablePin1 = enable1;
  _phasePin1 = phase1;
  _enablePin2 = enable2;
  _phasePin2 = phase2;
  _modePin = mode;
  
  _turnTime180 = 0;
  _turnSpeed = 150;
  _driveSpeed = 255;
  
  setDriveDirection(HIGH);
  digitalWrite(_modePin, HIGH); // put motor controller in phase/enable mode
}

MotorControl::~MotorControl() {

}

void MotorControl::setDriveDirection(bool forward) {
  digitalWrite(_phasePin1, forward); // switch turn direction
  digitalWrite(_phasePin2, forward); // switch turn direction
}

void MotorControl::setDriveSpeed(int speed) {
	_driveSpeed = speed;
}

void MotorControl::drive(bool forward) {
  //drive(forward, 170);
	setDriveDirection(forward);

	analogWrite(_enablePin1, _driveSpeed);	// sets the value (range from 0 to 255)
	analogWrite(_enablePin2, _driveSpeed);	// sets the value (range from 0 to 255)
}

void MotorControl::drive(bool forward, int accelTime) {
  setDriveDirection(forward);
  double delayTime = accelTime / 17.0;
  // turn harder from min to max in increments of 5 points:
  for(double fadeValue = 0 ; fadeValue <= _driveSpeed; fadeValue += _driveSpeed / 17.0) { 
    analogWrite(_enablePin1, fadeValue); // sets the value (range from 0 to 255) 
    analogWrite(_enablePin2, fadeValue); // sets the value (range from 0 to 255)       
    delay(delayTime); // wait for 30 milliseconds to see the effect                           
  } 
  
  analogWrite(_enablePin1, _driveSpeed);
  analogWrite(_enablePin2, _driveSpeed);
}

void MotorControl::stopMotors() {
  stopMotors(170);
}

void MotorControl::stopMotors(int stopTime) {
  double delayTime = stopTime / 17.0;
  // turn harder from min to max in increments of 5 points:
  for(int fadeValue = _driveSpeed ; fadeValue >= 0; fadeValue -= _driveSpeed / 17.0) { 
    analogWrite(_enablePin1, fadeValue); // sets the value (range from 0 to 255) 
    analogWrite(_enablePin2, fadeValue); // sets the value (range from 0 to 255)       
    delay(delayTime); // wait for 30 milliseconds to see the effect                           
  } 
  
  analogWrite(_enablePin1, 0);
  analogWrite(_enablePin2, 0);
}

void MotorControl::setTurnSpeed(int speed) {
	_turnSpeed = speed;
}

void MotorControl::setTurnTime(double time) {
	_turnTime180 = time;
}

void MotorControl::turn(bool left) {
  digitalWrite(_phasePin1, left); // switch turn direction
  digitalWrite(_phasePin2, !left); // switch turn direction
  
  analogWrite(_enablePin1, _turnSpeed); // sets the value (range from 0 to 255) 
  analogWrite(_enablePin2, _turnSpeed); // sets the value (range from 0 to 255)       
}

void MotorControl::turn(bool left, double angle) {
	if (_turnTime180 != 0) {
		double delayTime = angle / 180 * _turnTime180;
	
		digitalWrite(_phasePin1, left); // switch turn direction
		digitalWrite(_phasePin2, !left); // switch turn direction
	  
		analogWrite(_enablePin1, _turnSpeed); // sets the value (range from 0 to 255) 
		analogWrite(_enablePin2, _turnSpeed); // sets the value (range from 0 to 255)
		
		delay(delayTime);
	}
}
